classdef my_trajectory  % 
   properties
       startpos = [28.1213,112.5931,0];
       trj_option = 1;
       trj
       PT
       AT
       vel
   end
   methods
       %构造函数
       function obj = my_trajectory(lat,lon,h,timerange,option)
            glvs
            ts = 1;
            glv.pos0=[lat*glv.deg;lon*glv.deg;h];
            avp0 = [[0;0;0]; [0;0;0]; glv.pos0];
            obj.startpos = [lat,lon,h];
            xxx = [];
            obj.trj_option = option;
            seg = genarate_trajectory(obj.trj_option,xxx);
            trj = trjsimu(avp0, seg.wat, ts, 1);
            simu_LL = trj.avp(:,7:8)*180/pi;
            simu_AT = trj.avp(:,9:10);
            simu_position = [simu_LL,simu_AT];
            n = length(simu_position);
            trajectory = ones(n/(1/ts),4);
            j = 1;
            for i = 1:n
                if rem(simu_position(i,4),1) == 0
                    trajectory(j,:) = simu_position(i,:);
                    j = j+1;
                end
            end
            % startTime = datetime(2023,7,23,2,5,52);                     %起始时间
            % stopTime = startTime + seconds(3600);
            % sampleTime = seconds(1);
            % timerange = startTime:sampleTime:stopTime;
            % timerange = timerange';
            length_time = 1:min((j-1),length(timerange));
            % all_time = 1:length(timerange);
            Time = timerange';
            track = trajectory(:,1:3);
            simu_A = trj.avp(:,1:3);
            rotationOrder = 'ZYX';  % 依次绕Z、Y、X轴旋转
            % 欧拉角转四元数
            myquat = eul2quat(simu_A, rotationOrder);
            % PT_name = {'Time', 'Lat-Lon-Alt'};
            % AT_name = {'Time', 'Orientation'};
            if j-1<length(timerange)
                myquat_add = repmat(myquat(j-1,:), length(timerange)-(j-1), 1);
                track_add = repmat(track(j-1,:), length(timerange)-(j-1), 1);
                myquat = [myquat;myquat_add];
                track = [track;track_add];
                AT = table(Time, myquat);
                PT = table(Time, track);
            else
                AT = table(Time, myquat(length_time,:));
                PT = table(Time, track(length_time,:));
            end
            AT.Properties.VariableNames{2} = 'Orientation';
            PT.Properties.VariableNames{2} = 'Lat-Lon-Alt';
            obj.AT = AT;
            obj.PT = PT;
            obj.trj = trajectory;
            obj.vel = trj.avp(:,4:6);
            disp("轨迹已生成");
       end
       %函数申明
      obj = draw_trajectory(obj,mode,sat)
      obj = add_trajectory_noise(obj,noise_var)
      % seg = genarate_trajectory(option,xxx) 
      % obj = Doppler_position_using_Least_Squares(obj,ini_pos)
      % obj = Doppler_velocity_position_using_LS(obj,ini_pos,ini_vel)
      % obj = Doppler_position_using_EKF(obj,ini_pos,ini_vel)
      % obj = My_method_for_Doppler_position_based_on_LS(obj,ini_pos);
      % obj = My_method_for_Doppler_velocity_position_based_on_LS(obj,ini_pos,inivel);
      % obj = error_analysis(obj,coordinate);
   end    
end
